/*
 *  createCamera.cpp
 *  Ball and Plate
 *
 *  Created by Kenn Sebesta on 2008-01-23.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "createCamera.h"
extern const char* cameraLib;

void setupCamera(CamHandle *camFeed) {
    // Set up camera
    if (!strcmp(cameraLib,"opencv")){
        camFeed->opencv = cvCaptureFromCAM( CV_CAP_ANY );
        // Check that the camera exists
        if( !camFeed->opencv ) {
            fprintf( stderr, "ERROR: video input is NULL. \n"
                    "If usingLinux, check that you have read/write permissions on /dev/video1394/* and /dev/raw1394. \n" );
            exit(1);
        }
        // cvSetCaptureProperty( camFeed->opencv, CV_CAP_PROP_FRAME_WIDTH, 640 ); // THESE DON'T WORK RIGHT
        // cvSetCaptureProperty( camFeed->opencv, CV_CAP_PROP_FRAME_HEIGHT, 480 );
    }
    #ifdef CAMWIRE
    else if(!strcmp(cameraLib, "camwire")){
        camFeed->camwire = cwCaptureFromDCAM();
    }
    #endif
        
    return;
}

void queryCamera( IplImage** camFrame, CamHandle camFeed ) {
    if (!strcmp(cameraLib,"opencv")){
        *camFrame = cvQueryFrame( camFeed.opencv ); //DO NOT RELEASE THIS IMAGE
    }
    #ifdef CAMWIRE    
    else if(!strcmp(cameraLib, "camwire")){
        void *capturebuffer = NULL;
        
        // Get and the next frame. Poll to keep from getting blocked.
//        if (camwire_point_next_frame_poll( camFeed.camwire, (void**)(*camFrame)->imageData, NULL ) != CAMWIRE_SUCCESS)
        if (camwire_point_next_frame( camFeed.camwire, &capturebuffer, NULL ) != CAMWIRE_SUCCESS)
            fprintf( stderr, "Could not point to the next frame." );
        memcpy( (*camFrame)->imageData, capturebuffer, (*camFrame)->width*(*camFrame)->height ); //Would like to replace this memcpy with the above code, but not having much luck.
        
        struct timespec timestamp;
        struct timeval tv_received;
        double start_time, received_time, trigger_time;
        gettimeofday( &tv_received, NULL );
        camwire_get_timestamp( camFeed.camwire, &timestamp);
        trigger_time = timestamp.tv_sec + 1.0e-9*timestamp.tv_nsec;
        received_time = tv_received.tv_sec + 1e-6*tv_received.tv_usec;
        
        
        camwire_unpoint_frame( camFeed.camwire );
        camwire_flush_framebuffers(camFeed.camwire, 100, NULL, NULL);
//        printf("Retard est de %fs\n", received_time - trigger_time );
    }
    #endif
    
    // Check to make sure the camera passed the image.
    if ( !(*camFrame) ) {
        fprintf(stderr, "ERROR: camFrame is null. Exiting...\n");
        exit(1);
    }
    return;
}

void releaseCamera( CamHandle camFeed ) {
    if (!strcmp(cameraLib,"opencv")){
        cvReleaseCapture( &camFeed.opencv );
    }
    #ifdef CAMWIRE
    else if(!strcmp(cameraLib, "camwire")){
        camwire_set_run_stop(camFeed.camwire, 0);
    }
    #endif
    fprintf(stderr, "Camera successfully released. Exiting...\n");
}
